/**
 * @brief 存放轨迹相关代码
 * @author GL
 * @date 2021/11/19
 * @copyright HITCRT VISION 2021
 */ 
// #include <vector>
#include <deque>

namespace hitcrt
{
    //轨迹状态，0代表新增，1代表关联成功，2代表短时间内丢失(保留),3代表彻底丢失
    //0代表不确定，1代表确定(包括更新和未更新)，2代表丢失(长时间未更新)
    enum class TrjStatus
    {
        // CREATE = 0,UPDATE = 1,RESERVED =2,MISSED = 3
        TENTATIVE = 0,CONFIRMED = 1,MISSED = 2
    };


    //轨迹类
    template <class T>
    class Trajectory
    {
    public:
        Trajectory()
        {
            this->id = 0;//默认轨迹id为0
            this->status = TrjStatus::TENTATIVE;//新建轨迹
            max_trj_length = 10;//轨迹长度默认为10
            this->reset();
            this->missed_cnt_thre = 5;//默认5次找不到视作丢失
            this->missed_time_thre = 0;//暂时没用
            this->confirm_num_thre = 5;//置信阈值
        }

        Trajectory(int _id,int _max_length)
        {
            this->id = _id;
            this->status = TrjStatus::TENTATIVE;//新建轨迹
            max_trj_length = _max_length;
            this->reset();
            this->missed_cnt_thre = 5;//默认5次找不到视作丢失
            this->missed_time_thre = 0;//暂时没用
            this->confirm_num_thre = 5;//置信阈值
        }
        Trajectory(int _id,int _max_length,int _max_missed_cnt)
        {
            this->id = _id;
            this->status = TrjStatus::TENTATIVE;//新建轨迹
            max_trj_length = _max_length;
            this->reset();
            this->missed_cnt_thre = _max_missed_cnt;
            this->missed_time_thre = 0;//暂时没用
            this->confirm_num_thre = 5;//置信阈值
        }

        ~Trajectory(){};

        /**
         * @brief rest主要清除状态和计数
         */ 
        void reset()
        {
            this->data.clear();//清空数据
            this->status = TrjStatus::TENTATIVE;//新建轨迹
            this->miss_cnt = 0;//丢失计数重置
            this->hit_num = 0;//匹配次数重置
        }

        /**
         * @brief 根据元素 + 时间戳进行更新
         */ 
        void update(const T &_element,const double _time_stamp)
        {
            this->current_time = _time_stamp;//更新时间
            this->miss_cnt = 0;//丢失计数重置
            this->hit_num++;
            if (this->hit_num >= this->confirm_num_thre)
            {
                this->status = TrjStatus::CONFIRMED;
            }
            //更新数据
            if (this->data.size()==this->max_trj_length)
            { //如果已经满了
                auto ele = this->data.pop_front();//移除头部元素
                this->data.push_back(T);//加入新元素
            }
            else
            { //如果未满
                this->data.push_back(T);//加入新元素
            }
            this->missed_time = _time_stamp;//
        }

        /**
         * @brief 对无新元素的轨迹更新
         * 
         */ 
        void update(const double _time_stamp)
        {
            this->current_time = _time_stamp;//更新时间
            if (this->status == TrjStatus::TENTATIVE)
            { //如果为新轨迹
                return ;
            }
            else if (this->status = TrjStatus::MISSED)
            { //如果为丢失，则重置
                this->reset();
            }
            this->miss_cnt++;//计数
            if (this->miss_cnt >= this->missed_cnt_thre)
            { //如果超过丢失阈值
                this->status = TrjStatus::MISSED;
            }
            return ;
        }

        void set_threshold_cnt(int _threshold)
        { //设置丢失计数阈值
            this->missed_cnt_thre = _threshold;
        }

        void set_threshold_time(double _threshold)
        { //设置丢失时间阈值
            this->missed_time_thre = _threshold;
        }

        void set_max_length(int _length)
        { //设置轨迹最大长度
            this->max_trj_length = _length;
        }

        void set_confirm_threshold(int _threshold)
        { 
            this->confirm_num_thre = _threshold;
        }

        /**
         * @brief 输出轨迹debug信息,最好重载<<运算符
         * @todo 加入对data的输出
         */ 
        void print_debug_string()
        {
            std::cout<<"[Trj "<<this->id<<"]:status="<<status<<", miss cnt="<<this->miss_cnt<<std::endl;
        }

    // private:

    public:
        std::deque<T> data;//轨迹信息存储 

    private:
        int id;//轨迹id
        TrjStatus status;//轨迹当前状态
        int max_trj_length;//轨迹最大长度

        //参考deepsort,只有匹配次数超过一定阈值才认为轨迹是可信的
        int hit_num;//匹配成功次数
        int confirm_num_thre;//可信阈值

        
        //根据计数器确定轨迹状态
        int miss_cnt;//丢失目标计数，超过阈值会将轨迹状态设置为MISSED，并进行reset
        int missed_cnt_thre;//丢失目标计数阈值

        //根据时间确定轨迹状态
        double current_time;//当前最新轨迹对应系统时间
        double missed_time;//丢失时刻对应系统时间
        double missed_time_thre;//丢失时间阈值

    };

}